//////////////////////////////////////////////////////////////////////
//
//	ZETeam source code 
//	Copyright (c) ZETeam 2011-..
//	
//	File: CVector.h
//  Description: Vector classes. Based on Id Software source code.
// 
//	History:
//	- February 26, 2011: Created by Max Streltsov
//
//////////////////////////////////////////////////////////////////////

#ifndef CVECTOR_H
#define CVECTOR_H

#include "CMath.h"


#define VECTOR_EPSILON		0.001f

class CAngles;
class CPolar3;
class CMat3;


//===============================================================
//
//	CVec2 - 2D vector
//
//===============================================================

class CVec2
{
public:

	float x;
	float y;

	CVec2();
	explicit CVec2(const float x, const float y);

	void set(const float x, const float y);
	void zero();

	float			operator[](int index) const;
	float &			operator[](int index);

	CVec2			operator-() const;
	float			operator*(const CVec2 &a) const;
	CVec2			operator*(const float a) const;
	CVec2			operator/(const float a) const;
	CVec2			operator+(const CVec2 &a) const;
	CVec2			operator-(const CVec2 &a) const;
	CVec2 &			operator+=(const CVec2 &a);
	CVec2 &			operator-=(const CVec2 &a);
	CVec2 &			operator/=(const CVec2 &a);
	CVec2 &			operator/=(const float a);
	CVec2 &			operator*=(const float a);

	friend CVec2	operator*(const float a, const CVec2 & b);

	bool			compare(const CVec2 &a) const;							// exact compare, no epsilon
	bool			compare(const CVec2 &a, const float epsilon) const;		// compare with epsilon
	bool			operator==(const CVec2 &a) const;						// exact compare, no epsilon
	bool			operator!=(const CVec2 &a) const;						// exact compare, no epsilon

	float			length() const;
	float			lengthFast() const;
	float			lengthSqr() const;
	float			normalize();			// returns length
	float			normalizeFast();		// returns length
	CVec2 &			truncate(float new_length);	// cap length
	void			clamp(const CVec2 &min, const CVec2 &max);
	void			snap();				// snap to closest integer value
	void			snapInt();			// snap towards integer (floor)

	int				getDimension() const;

	const float *	toFloatPtr() const;
	float *			toFloatPtr();
	//const char *	toString(int precision = 2) const;

	void			lerp(const CVec2 &v1, const CVec2 &v2, const float l);
};

extern CVec2 vec2_origin;
#define vec2_zero vec2_origin

Z_INLINE CVec2::CVec2()
{
	
};

Z_INLINE CVec2::CVec2(const float x, const float y)
{
	this->x = x;
	this->y = y;
};

Z_INLINE void CVec2::set(const float x, const float y)
{
	this->x = x;
	this->y = y;
};

Z_INLINE void CVec2::zero(void)
{
	x = y = 0.0f;
};

Z_INLINE bool CVec2::compare(const CVec2 &a) const
{
	return ((x == a.x) && (y == a.y));
};

Z_INLINE bool CVec2::compare(const CVec2 &a, const float epsilon) const
{
	if (CZMath::Fabs( x - a.x ) > epsilon)
	{
		return false;
	}
			
	if (CZMath::Fabs(y - a.y ) > epsilon)
	{
		return false;
	}

	return true;
};

Z_INLINE bool CVec2::operator==(const CVec2 &a) const
{
	return compare(a);
};

Z_INLINE bool CVec2::operator!=(const CVec2 &a) const
{
	return !compare(a);
};

Z_INLINE float CVec2::operator[](int index) const
{
	return (&x)[index];
};

Z_INLINE float & CVec2::operator[](int index)
{
	return (&x)[index];
};

Z_INLINE float CVec2::length() const
{
	return (float)CZMath::Sqrt(x * x + y * y);
};

Z_INLINE float CVec2::lengthFast() const
{
	float sqrLength = x * x + y * y;

	return sqrLength * CZMath::revSqrt(sqrLength);
};

Z_INLINE float CVec2::lengthSqr() const
{
	return (x * x + y * y);
};

Z_INLINE float CVec2::normalize()
{
	float sqrLength, invLength;

	sqrLength = x * x + y * y;
	invLength = CZMath::InvSqrt(sqrLength);
	x *= invLength;
	y *= invLength;

	return invLength * sqrLength;
};

Z_INLINE float CVec2::normalizeFast(void)
{
	float lengthSqr, invLength;

	lengthSqr = x * x + y * y;
	invLength = CZMath::revSqrt(lengthSqr);
	x *= invLength;
	y *= invLength;
	return invLength * lengthSqr;
};

Z_INLINE CVec2 & CVec2::truncate(float new_length)
{
	float length2;
	float ilength;

	if (!new_length)
	{
		zero();
	}
	else
	{
		length2 = lengthSqr();

		if (length2 > new_length * new_length)
		{
			ilength = new_length * CZMath::InvSqrt(length2);
			x *= ilength;
			y *= ilength;
		}
	}

	return *this;
};

Z_INLINE void CVec2::clamp(const CVec2 &min, const CVec2 &max)
{
	if (x < min.x)
	{
		x = min.x;
	}
	else if (x > max.x)
	{
		x = max.x;
	}

	if (y < min.y)
	{
		y = min.y;
	}
	else if (y > max.y)
	{
		y = max.y;
	}
}

Z_INLINE void CVec2::snap(void)
{
	x = floor(x + 0.5f);
	y = floor(y + 0.5f);
}

Z_INLINE void CVec2::snapInt(void)
{
	x = float(int(x));
	y = float(int(y));
}

Z_INLINE CVec2 CVec2::operator-() const
{
	return CVec2(-x, -y);
}
	
Z_INLINE CVec2 CVec2::operator-(const CVec2 &a) const
{
	return CVec2(x - a.x, y - a.y);
}

Z_INLINE float CVec2::operator*(const CVec2 &a) const
{
	return x * a.x + y * a.y;
}

Z_INLINE CVec2 CVec2::operator*(const float a) const
{
	return CVec2(x * a, y * a);
}

Z_INLINE CVec2 CVec2::operator/(const float a) const
{
	float inva = 1.0f / a;
	return CVec2(x * inva, y * inva);
}

Z_INLINE CVec2 operator*(const float a, const CVec2 &b)
{
	return CVec2(b.x * a, b.y * a);
}

Z_INLINE CVec2 CVec2::operator+(const CVec2 &a) const
{
	return CVec2(x + a.x, y + a.y);
}

Z_INLINE CVec2 & CVec2::operator+=(const CVec2 &a)
{
	x += a.x;
	y += a.y;

	return *this;
}

Z_INLINE CVec2 & CVec2::operator/=(const CVec2 &a)
{
	x /= a.x;
	y /= a.y;

	return *this;
}

Z_INLINE CVec2 & CVec2::operator/=(const float a)
{
	float inva = 1.0f / a;
	x *= inva;
	y *= inva;

	return *this;
}

Z_INLINE CVec2 & CVec2::operator-=(const CVec2 &a)
{
	x -= a.x;
	y -= a.y;

	return *this;
}

Z_INLINE CVec2 & CVec2::operator*=(const float a)
{
	x *= a;
	y *= a;

	return *this;
}

Z_INLINE int CVec2::getDimension() const
{
	return 2;
}

Z_INLINE const float *CVec2::toFloatPtr() const
{
	return &x;
}

Z_INLINE float *CVec2::toFloatPtr()
{
	return &x;
}


//===============================================================
//
//	CVec3 - 3D vector
//
//===============================================================

class CVec3 {
public:	
	float			x;
	float			y;
	float			z;

					CVec3();
					explicit CVec3(const float x, const float y, const float z);

	void 			set(const float x, const float y, const float z);
	void			zero();

	float			operator[](const int index) const;
	float &			operator[](const int index);
	CVec3			operator-() const;
	CVec3 &			operator=(const CVec3 &a);		// required because of a msvc 6 & 7 bug
	float			operator*(const CVec3 &a) const;
	CVec3			operator*(const float a) const;
	CVec3			operator/(const float a) const;
	CVec3			operator+(const CVec3 &a) const;
	CVec3			operator-(const CVec3 &a) const;
	CVec3 &		operator+=(const CVec3 &a);
	CVec3 &		operator-=(const CVec3 &a);
	CVec3 &		operator/=(const CVec3 &a);
	CVec3 &		operator/=(const float a);
	CVec3 &		operator*=(const float a);

	friend CVec3	operator*( const float a, const CVec3 b );

	bool			compare( const CVec3 &a ) const;							// exact compare, no epsilon
	bool			compare( const CVec3 &a, const float epsilon ) const;		// compare with epsilon
	bool			operator==(	const CVec3 &a ) const;						// exact compare, no epsilon
	bool			operator!=(	const CVec3 &a ) const;						// exact compare, no epsilon

	bool			fixDegenerateNormal( void );	// fix degenerate axial cases
	bool			fixDenormals( void );			// change tiny numbers to zero

	CVec3			cross( const CVec3 &a ) const;
	CVec3 &			cross( const CVec3 &a, const CVec3 &b );
	float			length( void ) const;
	float			lengthSqr( void ) const;
	float			lengthFast( void ) const;
	float			normalize( void );				// returns length
	float			normalizeFast( void );			// returns length
	CVec3 &		truncate( float new_length );		// cap length
	void			clamp( const CVec3 &min, const CVec3 &max );
	void			snap( void );					// snap to closest integer value
	void			snapInt( void );				// snap towards integer (floor)

	int				getDimension( void ) const;

	float			toYaw( void ) const;
	float			toPitch( void ) const;
	CAngles		toAngles( void ) const;
	CPolar3		toPolar( void ) const;
	CMat3			toMat3( void ) const;		// vector should be normalized
	const CVec2 &	toVec2( void ) const;
	CVec2 &			toVec2( void );
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

	void			normalVectors( CVec3 &left, CVec3 &down ) const;	// vector should be normalized
	void			orthogonalBasis( CVec3 &left, CVec3 &up ) const;

	void			projectOntoPlane( const CVec3 &normal, const float overBounce = 1.0f );
	bool			projectAlongPlane( const CVec3 &normal, const float epsilon, const float overBounce = 1.0f );
	void			projectSelfOntoSphere( const float radius );

	void			lerp( const CVec3 &v1, const CVec3 &v2, const float l );
	void			sLerp( const CVec3 &v1, const CVec3 &v2, const float l );
};

extern CVec3 vec3_origin;
#define vec3_zero vec3_origin

Z_INLINE CVec3::CVec3( void )
{
}

Z_INLINE CVec3::CVec3( const float x, const float y, const float z )
{
	this->x = x;
	this->y = y;
	this->z = z;
}

Z_INLINE float CVec3::operator[]( const int index ) const
{
	return ( &x )[ index ];
}

Z_INLINE float &CVec3::operator[]( const int index )
{
	return ( &x )[ index ];
}

Z_INLINE void CVec3::set( const float x, const float y, const float z )
{
	this->x = x;
	this->y = y;
	this->z = z;
}

Z_INLINE void CVec3::zero( void )
{
	x = y = z = 0.0f;
}

Z_INLINE CVec3 CVec3::operator-() const
{
	return CVec3( -x, -y, -z );
}

Z_INLINE CVec3 &CVec3::operator=( const CVec3 &a )
{
	x = a.x;
	y = a.y;
	z = a.z;
	return *this;
}

Z_INLINE CVec3 CVec3::operator-( const CVec3 &a ) const
{
	return CVec3( x - a.x, y - a.y, z - a.z );
}

Z_INLINE float CVec3::operator*( const CVec3 &a ) const
{
	return x * a.x + y * a.y + z * a.z;
}

Z_INLINE CVec3 CVec3::operator*( const float a ) const
{
	return CVec3( x * a, y * a, z * a );
}

Z_INLINE CVec3 CVec3::operator/( const float a ) const
{
	float inva = 1.0f / a;
	return CVec3( x * inva, y * inva, z * inva );
}

Z_INLINE CVec3 operator*( const float a, const CVec3 b )
{
	return CVec3( b.x * a, b.y * a, b.z * a );
}

Z_INLINE CVec3 CVec3::operator+( const CVec3 &a ) const
{
	return CVec3( x + a.x, y + a.y, z + a.z );
}

Z_INLINE CVec3 &CVec3::operator+=( const CVec3 &a )
{
	x += a.x;
	y += a.y;
	z += a.z;

	return *this;
}

Z_INLINE CVec3 &CVec3::operator/=( const CVec3 &a )
{
	x /= a.x;
	y /= a.y;
	z /= a.z;

	return *this;
}

Z_INLINE CVec3 &CVec3::operator/=( const float a )
{
	float inva = 1.0f / a;
	x *= inva;
	y *= inva;
	z *= inva;

	return *this;
}

Z_INLINE CVec3 &CVec3::operator-=( const CVec3 &a )
{
	x -= a.x;
	y -= a.y;
	z -= a.z;

	return *this;
}

Z_INLINE CVec3 &CVec3::operator*=( const float a )
{
	x *= a;
	y *= a;
	z *= a;

	return *this;
}

Z_INLINE bool CVec3::compare( const CVec3 &a ) const
{
	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
}

Z_INLINE bool CVec3::compare( const CVec3 &a, const float epsilon ) const
{
	if ( CZMath::Fabs( x - a.x ) > epsilon )
	{
		return false;
	}
			
	if ( CZMath::Fabs( y - a.y ) > epsilon )
	{
		return false;
	}

	if ( CZMath::Fabs( z - a.z ) > epsilon )
	{
		return false;
	}

	return true;
}

Z_INLINE bool CVec3::operator==( const CVec3 &a ) const
{
	return compare( a );
}

Z_INLINE bool CVec3::operator!=( const CVec3 &a ) const
{
	return !compare( a );
}

Z_INLINE float CVec3::normalizeFast( void )
{
	float sqrLength, invLength;

	sqrLength = x * x + y * y + z * z;
	invLength = CZMath::revSqrt( sqrLength );
	x *= invLength;
	y *= invLength;
	z *= invLength;
	return invLength * sqrLength;
}

Z_INLINE bool CVec3::fixDegenerateNormal( void )
{
	if ( x == 0.0f ) {
		if ( y == 0.0f ) {
			if ( z > 0.0f ) {
				if ( z != 1.0f ) {
					z = 1.0f;
					return true;
				}
			} else {
				if ( z != -1.0f ) {
					z = -1.0f;
					return true;
				}
			}
			return false;
		} else if ( z == 0.0f ) {
			if ( y > 0.0f ) {
				if ( y != 1.0f ) {
					y = 1.0f;
					return true;
				}
			} else {
				if ( y != -1.0f ) {
					y = -1.0f;
					return true;
				}
			}
			return false;
		}
	} else if ( y == 0.0f ) {
		if ( z == 0.0f ) {
			if ( x > 0.0f ) {
				if ( x != 1.0f ) {
					x = 1.0f;
					return true;
				}
			} else {
				if ( x != -1.0f ) {
					x = -1.0f;
					return true;
				}
			}
			return false;
		}
	}

	if ( CZMath::Fabs( x ) == 1.0f ) {
		if ( y != 0.0f || z != 0.0f ) {
			y = z = 0.0f;
			return true;
		}
		return false;
	} else if ( CZMath::Fabs( y ) == 1.0f ) {
		if ( x != 0.0f || z != 0.0f ) {
			x = z = 0.0f;
			return true;
		}
		return false;
	} else if ( CZMath::Fabs( z ) == 1.0f ) {
		if ( x != 0.0f || y != 0.0f ) {
			x = y = 0.0f;
			return true;
		}
		return false;
	}
	return false;
}

Z_INLINE bool CVec3::fixDenormals( void ) {
	bool denormal = false;
	if ( fabs( x ) < 1e-30f ) {
		x = 0.0f;
		denormal = true;
	}
	if ( fabs( y ) < 1e-30f ) {
		y = 0.0f;
		denormal = true;
	}
	if ( fabs( z ) < 1e-30f ) {
		z = 0.0f;
		denormal = true;
	}
	return denormal;
}

Z_INLINE CVec3 CVec3::cross( const CVec3 &a ) const {
	return CVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
}

Z_INLINE CVec3 &CVec3::cross( const CVec3 &a, const CVec3 &b ) {
	x = a.y * b.z - a.z * b.y;
	y = a.z * b.x - a.x * b.z;
	z = a.x * b.y - a.y * b.x;

	return *this;
}

Z_INLINE float CVec3::length( void ) const {
	return ( float )CZMath::Sqrt( x * x + y * y + z * z );
}

Z_INLINE float CVec3::lengthSqr( void ) const {
	return ( x * x + y * y + z * z );
}

Z_INLINE float CVec3::lengthFast( void ) const {
	float sqrLength;

	sqrLength = x * x + y * y + z * z;
	return sqrLength * CZMath::revSqrt( sqrLength );
}

Z_INLINE float CVec3::normalize( void ) {
	float sqrLength, invLength;

	sqrLength = x * x + y * y + z * z;
	invLength = CZMath::InvSqrt( sqrLength );
	x *= invLength;
	y *= invLength;
	z *= invLength;
	return invLength * sqrLength;
}

Z_INLINE CVec3 &CVec3::truncate( float new_length ) {
	float length2;
	float ilength;

	if ( !new_length ) {
		zero();
	}
	else {
		length2 = lengthSqr();
		if ( length2 > new_length * new_length ) {
			ilength = new_length * CZMath::InvSqrt( length2 );
			x *= ilength;
			y *= ilength;
			z *= ilength;
		}
	}

	return *this;
}

Z_INLINE void CVec3::clamp( const CVec3 &min, const CVec3 &max ) {
	if ( x < min.x ) {
		x = min.x;
	} else if ( x > max.x ) {
		x = max.x;
	}
	if ( y < min.y ) {
		y = min.y;
	} else if ( y > max.y ) {
		y = max.y;
	}
	if ( z < min.z ) {
		z = min.z;
	} else if ( z > max.z ) {
		z = max.z;
	}
}

Z_INLINE void CVec3::snap( void ) {
	x = floor( x + 0.5f );
	y = floor( y + 0.5f );
	z = floor( z + 0.5f );
}

Z_INLINE void CVec3::snapInt( void ) {
	x = float( int( x ) );
	y = float( int( y ) );
	z = float( int( z ) );
}

Z_INLINE int CVec3::getDimension( void ) const {
	return 3;
}

Z_INLINE const CVec2 &CVec3::toVec2( void ) const {
	return *reinterpret_cast<const CVec2 *>(this);
}

Z_INLINE CVec2 &CVec3::toVec2( void ) {
	return *reinterpret_cast<CVec2 *>(this);
}

Z_INLINE const float *CVec3::toFloatPtr( void ) const {
	return &x;
}

Z_INLINE float *CVec3::toFloatPtr( void ) {
	return &x;
}

Z_INLINE void CVec3::normalVectors( CVec3 &left, CVec3 &down ) const {
	float d;

	d = x * x + y * y;
	if ( !d ) {
		left[0] = 1;
		left[1] = 0;
		left[2] = 0;
	} else {
		d = CZMath::InvSqrt( d );
		left[0] = -y * d;
		left[1] = x * d;
		left[2] = 0;
	}
	down = left.cross( *this );
}

Z_INLINE void CVec3::orthogonalBasis( CVec3 &left, CVec3 &up ) const {
	float l, s;

	if ( CZMath::Fabs( z ) > 0.7f ) {
		l = y * y + z * z;
		s = CZMath::InvSqrt( l );
		up[0] = 0;
		up[1] = z * s;
		up[2] = -y * s;
		left[0] = l * s;
		left[1] = -x * up[2];
		left[2] = x * up[1];
	}
	else {
		l = x * x + y * y;
		s = CZMath::InvSqrt( l );
		left[0] = -y * s;
		left[1] = x * s;
		left[2] = 0;
		up[0] = -z * left[1];
		up[1] = z * left[0];
		up[2] = l * s;
	}
}

Z_INLINE void CVec3::projectOntoPlane( const CVec3 &normal, const float overBounce ) {
	float backoff;
	
	backoff = *this * normal;
	
	if ( overBounce != 1.0 ) {
		if ( backoff < 0 ) {
			backoff *= overBounce;
		} else {
			backoff /= overBounce;
		}
	}

	*this -= backoff * normal;
}

Z_INLINE bool CVec3::projectAlongPlane( const CVec3 &normal, const float epsilon, const float overBounce ) {
	CVec3 v_cross;
	float len;

	v_cross = this->cross( normal ).cross( (*this) );
	// normalize so a fixed epsilon can be used
	v_cross.normalize();
	len = normal * v_cross;
	if ( CZMath::Fabs( len ) < epsilon ) {
		return false;
	}
	v_cross *= overBounce * ( normal * (*this) ) / len;
	(*this) -= v_cross;
	return true;
}


//===============================================================
//
//	CVec4 - 4D vector
//
//===============================================================

class CVec4 {
public:	
	float			x;
	float			y;
	float			z;
	float			w;

					CVec4( void );
					explicit CVec4( const float x, const float y, const float z, const float w );

	void 			set( const float x, const float y, const float z, const float w );
	void			zero( void );

	float			operator[]( const int index ) const;
	float &			operator[]( const int index );
	CVec4			operator-() const;
	float			operator*( const CVec4 &a ) const;
	CVec4			operator*( const float a ) const;
	CVec4			operator/( const float a ) const;
	CVec4			operator+( const CVec4 &a ) const;
	CVec4			operator-( const CVec4 &a ) const;
	CVec4 &		operator+=( const CVec4 &a );
	CVec4 &		operator-=( const CVec4 &a );
	CVec4 &		operator/=( const CVec4 &a );
	CVec4 &		operator/=( const float a );
	CVec4 &		operator*=( const float a );

	friend CVec4	operator*( const float a, const CVec4 b );

	bool			compare( const CVec4 &a ) const;							// exact compare, no epsilon
	bool			compare( const CVec4 &a, const float epsilon ) const;		// compare with epsilon
	bool			operator==(	const CVec4 &a ) const;						// exact compare, no epsilon
	bool			operator!=(	const CVec4 &a ) const;						// exact compare, no epsilon

	float			length( void ) const;
	float			lengthSqr( void ) const;
	float			normalize( void );			// returns length
	float			normalizeFast( void );		// returns length

	int				getDimension( void ) const;

	const CVec2 &	toVec2( void ) const;
	CVec2 &		toVec2( void );
	const CVec3 &	toVec3( void ) const;
	CVec3 &		toVec3( void );
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

	void			lerp( const CVec4 &v1, const CVec4 &v2, const float l );
};

extern CVec4 vec4_origin;
#define vec4_zero vec4_origin

Z_INLINE CVec4::CVec4( void ) {
}

Z_INLINE CVec4::CVec4( const float x, const float y, const float z, const float w ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Z_INLINE void CVec4::set( const float x, const float y, const float z, const float w ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->w = w;
}

Z_INLINE void CVec4::zero( void ) {
	x = y = z = w = 0.0f;
}

Z_INLINE float CVec4::operator[]( int index ) const {
	return ( &x )[ index ];
}

Z_INLINE float& CVec4::operator[]( int index ) {
	return ( &x )[ index ];
}

Z_INLINE CVec4 CVec4::operator-() const {
	return CVec4( -x, -y, -z, -w );
}

Z_INLINE CVec4 CVec4::operator-( const CVec4 &a ) const {
	return CVec4( x - a.x, y - a.y, z - a.z, w - a.w );
}

Z_INLINE float CVec4::operator*( const CVec4 &a ) const {
	return x * a.x + y * a.y + z * a.z + w * a.w;
}

Z_INLINE CVec4 CVec4::operator*( const float a ) const {
	return CVec4( x * a, y * a, z * a, w * a );
}

Z_INLINE CVec4 CVec4::operator/( const float a ) const {
	float inva = 1.0f / a;
	return CVec4( x * inva, y * inva, z * inva, w * inva );
}

Z_INLINE CVec4 operator*( const float a, const CVec4 b ) {
	return CVec4( b.x * a, b.y * a, b.z * a, b.w * a );
}

Z_INLINE CVec4 CVec4::operator+( const CVec4 &a ) const {
	return CVec4( x + a.x, y + a.y, z + a.z, w + a.w );
}

Z_INLINE CVec4 &CVec4::operator+=( const CVec4 &a ) {
	x += a.x;
	y += a.y;
	z += a.z;
	w += a.w;

	return *this;
}

Z_INLINE CVec4 &CVec4::operator/=( const CVec4 &a ) {
	x /= a.x;
	y /= a.y;
	z /= a.z;
	w /= a.w;

	return *this;
}

Z_INLINE CVec4 &CVec4::operator/=( const float a ) {
	float inva = 1.0f / a;
	x *= inva;
	y *= inva;
	z *= inva;
	w *= inva;

	return *this;
}

Z_INLINE CVec4 &CVec4::operator-=( const CVec4 &a ) {
	x -= a.x;
	y -= a.y;
	z -= a.z;
	w -= a.w;

	return *this;
}

Z_INLINE CVec4 &CVec4::operator*=( const float a ) {
	x *= a;
	y *= a;
	z *= a;
	w *= a;

	return *this;
}

Z_INLINE bool CVec4::compare( const CVec4 &a ) const {
	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && w == a.w );
}

Z_INLINE bool CVec4::compare( const CVec4 &a, const float epsilon ) const {
	if ( CZMath::Fabs( x - a.x ) > epsilon ) {
		return false;
	}
			
	if ( CZMath::Fabs( y - a.y ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( z - a.z ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( w - a.w ) > epsilon ) {
		return false;
	}

	return true;
}

Z_INLINE bool CVec4::operator==( const CVec4 &a ) const {
	return compare( a );
}

Z_INLINE bool CVec4::operator!=( const CVec4 &a ) const {
	return !compare( a );
}

Z_INLINE float CVec4::length( void ) const {
	return ( float )CZMath::Sqrt( x * x + y * y + z * z + w * w );
}

Z_INLINE float CVec4::lengthSqr( void ) const {
	return ( x * x + y * y + z * z + w * w );
}

Z_INLINE float CVec4::normalize( void ) {
	float sqrLength, invLength;

	sqrLength = x * x + y * y + z * z + w * w;
	invLength = CZMath::InvSqrt( sqrLength );
	x *= invLength;
	y *= invLength;
	z *= invLength;
	w *= invLength;
	return invLength * sqrLength;
}

Z_INLINE float CVec4::normalizeFast( void ) {
	float sqrLength, invLength;

	sqrLength = x * x + y * y + z * z + w * w;
	invLength = CZMath::revSqrt( sqrLength );
	x *= invLength;
	y *= invLength;
	z *= invLength;
	w *= invLength;
	return invLength * sqrLength;
}

Z_INLINE int CVec4::getDimension( void ) const {
	return 4;
}

Z_INLINE const CVec2 &CVec4::toVec2( void ) const {
	return *reinterpret_cast<const CVec2 *>(this);
}

Z_INLINE CVec2 &CVec4::toVec2( void ) {
	return *reinterpret_cast<CVec2 *>(this);
}

Z_INLINE const CVec3 &CVec4::toVec3( void ) const {
	return *reinterpret_cast<const CVec3 *>(this);
}

Z_INLINE CVec3 &CVec4::toVec3( void ) {
	return *reinterpret_cast<CVec3 *>(this);
}

Z_INLINE const float *CVec4::toFloatPtr( void ) const {
	return &x;
}

Z_INLINE float *CVec4::toFloatPtr( void ) {
	return &x;
}


//===============================================================
//
//	CVec5 - 5D vector
//
//===============================================================

class CVec5 {
public:
	float			x;
	float			y;
	float			z;
	float			s;
	float			t;

					CVec5( void );
					explicit CVec5( const CVec3 &xyz, const CVec2 &st );
					explicit CVec5( const float x, const float y, const float z, const float s, const float t );

	float			operator[]( int index ) const;
	float &			operator[]( int index );
	CVec5 &		operator=( const CVec3 &a );

	int				getDimension( void ) const;

	const CVec3 &	toVec3( void ) const;
	CVec3 &		toVec3( void );
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

	void			lerp( const CVec5 &v1, const CVec5 &v2, const float l );
};

extern CVec5 vec5_origin;
#define vec5_zero vec5_origin

Z_INLINE CVec5::CVec5( void ) {
}

Z_INLINE CVec5::CVec5( const CVec3 &xyz, const CVec2 &st ) {
	x = xyz.x;
	y = xyz.y;
	z = xyz.z;
	s = st[0];
	t = st[1];
}

Z_INLINE CVec5::CVec5( const float x, const float y, const float z, const float s, const float t ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->s = s;
	this->t = t;
}

Z_INLINE float CVec5::operator[]( int index ) const {
	return ( &x )[ index ];
}

Z_INLINE float& CVec5::operator[]( int index ) {
	return ( &x )[ index ];
}

Z_INLINE CVec5 &CVec5::operator=( const CVec3 &a ) { 
	x = a.x;
	y = a.y;
	z = a.z;
	s = t = 0;
	return *this;
}

Z_INLINE int CVec5::getDimension( void ) const {
	return 5;
}

Z_INLINE const CVec3 &CVec5::toVec3( void ) const {
	return *reinterpret_cast<const CVec3 *>(this);
}

Z_INLINE CVec3 &CVec5::toVec3( void ) {
	return *reinterpret_cast<CVec3 *>(this);
}

Z_INLINE const float *CVec5::toFloatPtr( void ) const {
	return &x;
}

Z_INLINE float *CVec5::toFloatPtr( void ) {
	return &x;
}


//===============================================================
//
//	CVec6 - 6D vector
//
//===============================================================

class CVec6 {
public:	
					CVec6( void );
					explicit CVec6( const float *a );
					explicit CVec6( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );

	void 			set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
	void			zero( void );

	float			operator[]( const int index ) const;
	float &			operator[]( const int index );
	CVec6			operator-() const;
	CVec6			operator*( const float a ) const;
	CVec6			operator/( const float a ) const;
	float			operator*( const CVec6 &a ) const;
	CVec6			operator-( const CVec6 &a ) const;
	CVec6			operator+( const CVec6 &a ) const;
	CVec6 &		operator*=( const float a );
	CVec6 &		operator/=( const float a );
	CVec6 &		operator+=( const CVec6 &a );
	CVec6 &		operator-=( const CVec6 &a );

	friend CVec6	operator*( const float a, const CVec6 b );

	bool			compare( const CVec6 &a ) const;							// exact compare, no epsilon
	bool			compare( const CVec6 &a, const float epsilon ) const;		// compare with epsilon
	bool			operator==(	const CVec6 &a ) const;						// exact compare, no epsilon
	bool			operator!=(	const CVec6 &a ) const;						// exact compare, no epsilon

	float			length( void ) const;
	float			lengthSqr( void ) const;
	float			normalize( void );			// returns length
	float			normalizeFast( void );		// returns length

	int				getDimension( void ) const;

	const CVec3 &	subVec3( int index ) const;
	CVec3 &		subVec3( int index );
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

private:
	float			p[6];
};

extern CVec6 vec6_origin;
#define vec6_zero vec6_origin
extern CVec6 vec6_infinity;

Z_INLINE CVec6::CVec6( void ) {
}

Z_INLINE CVec6::CVec6( const float *a ) {
	memcpy( p, a, 6 * sizeof( float ) );
}

Z_INLINE CVec6::CVec6( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
	p[0] = a1;
	p[1] = a2;
	p[2] = a3;
	p[3] = a4;
	p[4] = a5;
	p[5] = a6;
}

Z_INLINE CVec6 CVec6::operator-() const {
	return CVec6( -p[0], -p[1], -p[2], -p[3], -p[4], -p[5] );
}

Z_INLINE float CVec6::operator[]( const int index ) const {
	return p[index];
}

Z_INLINE float &CVec6::operator[]( const int index ) {
	return p[index];
}

Z_INLINE CVec6 CVec6::operator*( const float a ) const {
	return CVec6( p[0]*a, p[1]*a, p[2]*a, p[3]*a, p[4]*a, p[5]*a );
}

Z_INLINE float CVec6::operator*( const CVec6 &a ) const {
	return p[0] * a[0] + p[1] * a[1] + p[2] * a[2] + p[3] * a[3] + p[4] * a[4] + p[5] * a[5];
}

Z_INLINE CVec6 CVec6::operator/( const float a ) const {
	float inva;

	assert( a != 0.0f );
	inva = 1.0f / a;
	return CVec6( p[0]*inva, p[1]*inva, p[2]*inva, p[3]*inva, p[4]*inva, p[5]*inva );
}

Z_INLINE CVec6 CVec6::operator+( const CVec6 &a ) const {
	return CVec6( p[0] + a[0], p[1] + a[1], p[2] + a[2], p[3] + a[3], p[4] + a[4], p[5] + a[5] );
}

Z_INLINE CVec6 CVec6::operator-( const CVec6 &a ) const {
	return CVec6( p[0] - a[0], p[1] - a[1], p[2] - a[2], p[3] - a[3], p[4] - a[4], p[5] - a[5] );
}

Z_INLINE CVec6 &CVec6::operator*=( const float a ) {
	p[0] *= a;
	p[1] *= a;
	p[2] *= a;
	p[3] *= a;
	p[4] *= a;
	p[5] *= a;
	return *this;
}

Z_INLINE CVec6 &CVec6::operator/=( const float a ) {
	float inva;

	assert( a != 0.0f );
	inva = 1.0f / a;
	p[0] *= inva;
	p[1] *= inva;
	p[2] *= inva;
	p[3] *= inva;
	p[4] *= inva;
	p[5] *= inva;
	return *this;
}

Z_INLINE CVec6 &CVec6::operator+=( const CVec6 &a ) {
	p[0] += a[0];
	p[1] += a[1];
	p[2] += a[2];
	p[3] += a[3];
	p[4] += a[4];
	p[5] += a[5];
	return *this;
}

Z_INLINE CVec6 &CVec6::operator-=( const CVec6 &a ) {
	p[0] -= a[0];
	p[1] -= a[1];
	p[2] -= a[2];
	p[3] -= a[3];
	p[4] -= a[4];
	p[5] -= a[5];
	return *this;
}

Z_INLINE CVec6 operator*( const float a, const CVec6 b ) {
	return b * a;
}

Z_INLINE bool CVec6::compare( const CVec6 &a ) const {
	return ( ( p[0] == a[0] ) && ( p[1] == a[1] ) && ( p[2] == a[2] ) &&
			( p[3] == a[3] ) && ( p[4] == a[4] ) && ( p[5] == a[5] ) );
}

Z_INLINE bool CVec6::compare( const CVec6 &a, const float epsilon ) const {
	if ( CZMath::Fabs( p[0] - a[0] ) > epsilon ) {
		return false;
	}
			
	if ( CZMath::Fabs( p[1] - a[1] ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( p[2] - a[2] ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( p[3] - a[3] ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( p[4] - a[4] ) > epsilon ) {
		return false;
	}

	if ( CZMath::Fabs( p[5] - a[5] ) > epsilon ) {
		return false;
	}

	return true;
}

Z_INLINE bool CVec6::operator==( const CVec6 &a ) const {
	return compare( a );
}

Z_INLINE bool CVec6::operator!=( const CVec6 &a ) const {
	return !compare( a );
}

Z_INLINE void CVec6::set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
	p[0] = a1;
	p[1] = a2;
	p[2] = a3;
	p[3] = a4;
	p[4] = a5;
	p[5] = a6;
}

Z_INLINE void CVec6::zero( void ) {
	p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0.0f;
}

Z_INLINE float CVec6::length( void ) const {
	return ( float )CZMath::Sqrt( p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5] );
}

Z_INLINE float CVec6::lengthSqr( void ) const {
	return ( p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5] );
}

Z_INLINE float CVec6::normalize( void ) {
	float sqrLength, invLength;

	sqrLength = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5];
	invLength = CZMath::InvSqrt( sqrLength );
	p[0] *= invLength;
	p[1] *= invLength;
	p[2] *= invLength;
	p[3] *= invLength;
	p[4] *= invLength;
	p[5] *= invLength;
	return invLength * sqrLength;
}

Z_INLINE float CVec6::normalizeFast( void ) {
	float sqrLength, invLength;

	sqrLength = p[0] * p[0] + p[1] * p[1] + p[2] * p[2] + p[3] * p[3] + p[4] * p[4] + p[5] * p[5];
	invLength = CZMath::revSqrt( sqrLength );
	p[0] *= invLength;
	p[1] *= invLength;
	p[2] *= invLength;
	p[3] *= invLength;
	p[4] *= invLength;
	p[5] *= invLength;
	return invLength * sqrLength;
}

Z_INLINE int CVec6::getDimension( void ) const {
	return 6;
}

Z_INLINE const CVec3 &CVec6::subVec3( int index ) const {
	return *reinterpret_cast<const CVec3 *>(p + index * 3);
}

Z_INLINE CVec3 &CVec6::subVec3( int index ) {
	return *reinterpret_cast<CVec3 *>(p + index * 3);
}

Z_INLINE const float *CVec6::toFloatPtr( void ) const {
	return p;
}

Z_INLINE float *CVec6::toFloatPtr( void ) {
	return p;
}

/*  cant compile cause of memory functions */
/*
//===============================================================
//
//	CVecX - arbitrary sized vector
//
//  The vector lives on 16 byte aligned and 16 byte padded memory.
//
//	NOTE: due to the temporary memory pool CVecX cannot be used by multiple threads
//
//===============================================================

#define VECX_MAX_TEMP		1024
#define VECX_QUAD( x )		( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
#define VECX_CLEAREND()		int s = size; while( s < ( ( s + 3) & ~3 ) ) { p[s++] = 0.0f; }
#define VECX_ALLOCA( n )	( (float *) _alloca16( VECX_QUAD( n ) ) )
#define VECX_SIMD

class CVecX {
	friend class idMatX;

public:	
					CVecX( void );
					explicit CVecX( int length );
					explicit CVecX( int length, float *data );
					~CVecX( void );

	float			operator[]( const int index ) const;
	float &			operator[]( const int index );
	CVecX			operator-() const;
	CVecX &		operator=( const CVecX &a );
	CVecX			operator*( const float a ) const;
	CVecX			operator/( const float a ) const;
	float			operator*( const CVecX &a ) const;
	CVecX			operator-( const CVecX &a ) const;
	CVecX			operator+( const CVecX &a ) const;
	CVecX &		operator*=( const float a );
	CVecX &		operator/=( const float a );
	CVecX &		operator+=( const CVecX &a );
	CVecX &		operator-=( const CVecX &a );

	friend CVecX	operator*( const float a, const CVecX b );

	bool			Compare( const CVecX &a ) const;							// exact compare, no epsilon
	bool			Compare( const CVecX &a, const float epsilon ) const;		// compare with epsilon
	bool			operator==(	const CVecX &a ) const;						// exact compare, no epsilon
	bool			operator!=(	const CVecX &a ) const;						// exact compare, no epsilon

	void			SetSize( int size );
	void			ChangeSize( int size, bool makeZero = false );
	int				GetSize( void ) const { return size; }
	void			SetData( int length, float *data );
	void			Zero( void );
	void			Zero( int length );
	void			Random( int seed, float l = 0.0f, float u = 1.0f );
	void			Random( int length, int seed, float l = 0.0f, float u = 1.0f );
	void			Negate( void );
	void			Clamp( float min, float max );
	CVecX &		SwapElements( int e1, int e2 );

	float			Length( void ) const;
	float			LengthSqr( void ) const;
	CVecX			Normalize( void ) const;
	float			NormalizeSelf( void );

	int				GetDimension( void ) const;

	const CVec3 &	SubVec3( int index ) const;
	CVec3 &		SubVec3( int index );
	const CVec6 &	SubVec6( int index ) const;
	CVec6 &		SubVec6( int index );
	const float *	ToFloatPtr( void ) const;
	float *			ToFloatPtr( void );
	//const char *	ToString( int precision = 2 ) const;

private:
	int				size;					// size of the vector
	int				alloced;				// if -1 p points to data set with SetData
	float *			p;						// memory the vector is stored

	static float	temp[VECX_MAX_TEMP+4];	// used to store intermediate results
	static float *	tempPtr;				// pointer to 16 byte aligned temporary memory
	static int		tempIndex;				// index into memory pool, wraps around

private:
	void			SetTempSize( int size );
};


Z_INLINE CVecX::CVecX( void ) {
	size = alloced = 0;
	p = NULL;
}

Z_INLINE CVecX::CVecX( int length ) {
	size = alloced = 0;
	p = NULL;
	SetSize( length );
}

Z_INLINE CVecX::CVecX( int length, float *data ) {
	size = alloced = 0;
	p = NULL;
	SetData( length, data );
}

Z_INLINE CVecX::~CVecX( void ) {
	// if not temp memory
	if ( p && ( p < CVecX::tempPtr || p >= CVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
		Mem_Free16( p );
	}
}

Z_INLINE float CVecX::operator[]( const int index ) const {
	assert( index >= 0 && index < size );
	return p[index];
}

Z_INLINE float &CVecX::operator[]( const int index ) {
	assert( index >= 0 && index < size );
	return p[index];
}

Z_INLINE CVecX CVecX::operator-() const {
	int i;
	CVecX m;

	m.SetTempSize( size );
	for ( i = 0; i < size; i++ ) {
		m.p[i] = -p[i];
	}
	return m;
}

Z_INLINE CVecX &CVecX::operator=( const CVecX &a ) { 
	SetSize( a.size );
#ifdef VECX_SIMD
	//SIMDProcessor->Copy16( p, a.p, a.size );
#else
	memcpy( p, a.p, a.size * sizeof( float ) );
#endif
	CVecX::tempIndex = 0;
	return *this;
}

Z_INLINE CVecX CVecX::operator+( const CVecX &a ) const {
	CVecX m;

	assert( size == a.size );
	m.SetTempSize( size );
#ifdef VECX_SIMD
	SIMDProcessor->Add16( m.p, p, a.p, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		m.p[i] = p[i] + a.p[i];
	}
#endif
	return m;
}

Z_INLINE CVecX CVecX::operator-( const CVecX &a ) const {
	CVecX m;

	assert( size == a.size );
	m.SetTempSize( size );
#ifdef VECX_SIMD
	SIMDProcessor->Sub16( m.p, p, a.p, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		m.p[i] = p[i] - a.p[i];
	}
#endif
	return m;
}

Z_INLINE CVecX &CVecX::operator+=( const CVecX &a ) {
	assert( size == a.size );
#ifdef VECX_SIMD
	SIMDProcessor->AddAssign16( p, a.p, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		p[i] += a.p[i];
	}
#endif
	CVecX::tempIndex = 0;
	return *this;
}

Z_INLINE CVecX &CVecX::operator-=( const CVecX &a ) {
	assert( size == a.size );
#ifdef VECX_SIMD
	SIMDProcessor->SubAssign16( p, a.p, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		p[i] -= a.p[i];
	}
#endif
	CVecX::tempIndex = 0;
	return *this;
}

Z_INLINE CVecX CVecX::operator*( const float a ) const {
	CVecX m;

	m.SetTempSize( size );
#ifdef VECX_SIMD
	SIMDProcessor->Mul16( m.p, p, a, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		m.p[i] = p[i] * a;
	}
#endif
	return m;
}

Z_INLINE CVecX &CVecX::operator*=( const float a ) {
#ifdef VECX_SIMD
	SIMDProcessor->MulAssign16( p, a, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		p[i] *= a;
	}
#endif
	return *this;
}

Z_INLINE CVecX CVecX::operator/( const float a ) const {
	assert( a != 0.0f );
	return (*this) * ( 1.0f / a );
}

Z_INLINE CVecX &CVecX::operator/=( const float a ) {
	assert( a != 0.0f );
	(*this) *= ( 1.0f / a );
	return *this;
}

Z_INLINE CVecX operator*( const float a, const CVecX b ) {
	return b * a;
}

Z_INLINE float CVecX::operator*( const CVecX &a ) const {
	int i;
	float sum = 0.0f;

	assert( size == a.size );
	for ( i = 0; i < size; i++ ) {
		sum += p[i] * a.p[i];
	}
	return sum;
}

Z_INLINE bool CVecX::Compare( const CVecX &a ) const {
	int i;

	assert( size == a.size );
	for ( i = 0; i < size; i++ ) {
		if ( p[i] != a.p[i] ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CVecX::Compare( const CVecX &a, const float epsilon ) const {
	int i;

	assert( size == a.size );
	for ( i = 0; i < size; i++ ) {
		if ( CZMath::Fabs( p[i] - a.p[i] ) > epsilon ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CVecX::operator==( const CVecX &a ) const {
	return Compare( a );
}

Z_INLINE bool CVecX::operator!=( const CVecX &a ) const {
	return !Compare( a );
}

Z_INLINE void CVecX::SetSize( int newSize ) {
	int alloc = ( newSize + 3 ) & ~3;
	if ( alloc > alloced && alloced != -1 ) {
		if ( p ) {
			Mem_Free16( p );
		}
		p = (float *) Mem_Alloc16( alloc * sizeof( float ) );
		alloced = alloc;
	}
	size = newSize;
	VECX_CLEAREND();
}

Z_INLINE void CVecX::ChangeSize( int newSize, bool makeZero ) {
	int alloc = ( newSize + 3 ) & ~3;
	if ( alloc > alloced && alloced != -1 ) {
		float *oldVec = p;
		p = (float *) Mem_Alloc16( alloc * sizeof( float ) );
		alloced = alloc;
		if ( oldVec ) {
			for ( int i = 0; i < size; i++ ) {
				p[i] = oldVec[i];
			}
			Mem_Free16( oldVec );
		}
		if ( makeZero ) {
			// zero any new elements
			for ( int i = size; i < newSize; i++ ) {
				p[i] = 0.0f;
			}
		}
	}
	size = newSize;
	VECX_CLEAREND();
}

Z_INLINE void CVecX::SetTempSize( int newSize ) {

	size = newSize;
	alloced = ( newSize + 3 ) & ~3;
	assert( alloced < VECX_MAX_TEMP );
	if ( CVecX::tempIndex + alloced > VECX_MAX_TEMP ) {
		CVecX::tempIndex = 0;
	}
	p = CVecX::tempPtr + CVecX::tempIndex;
	CVecX::tempIndex += alloced;
	VECX_CLEAREND();
}

Z_INLINE void CVecX::SetData( int length, float *data ) {
	if ( p && ( p < CVecX::tempPtr || p >= CVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
		Mem_Free16( p );
	}
	assert( ( ( (int) data ) & 15 ) == 0 ); // data must be 16 byte aligned
	p = data;
	size = length;
	alloced = -1;
	VECX_CLEAREND();
}

Z_INLINE void CVecX::Zero( void ) {
#ifdef VECX_SIMD
	SIMDProcessor->Zero16( p, size );
#else
	memset( p, 0, size * sizeof( float ) );
#endif
}

Z_INLINE void CVecX::Zero( int length ) {
	SetSize( length );
#ifdef VECX_SIMD
	SIMDProcessor->Zero16( p, length );
#else
	memset( p, 0, size * sizeof( float ) );
#endif
}

Z_INLINE void CVecX::Random( int seed, float l, float u ) {
	int i;
	float c;
	idRandom rnd( seed );

	c = u - l;
	for ( i = 0; i < size; i++ ) {
		p[i] = l + rnd.RandomFloat() * c;
	}
}

Z_INLINE void CVecX::Random( int length, int seed, float l, float u ) {
	int i;
	float c;
	idRandom rnd( seed );

	SetSize( length );
	c = u - l;
	for ( i = 0; i < size; i++ ) {
		p[i] = l + rnd.RandomFloat() * c;
	}
}

Z_INLINE void CVecX::Negate( void ) {
#ifdef VECX_SIMD
	SIMDProcessor->Negate16( p, size );
#else
	int i;
	for ( i = 0; i < size; i++ ) {
		p[i] = -p[i];
	}
#endif
}

Z_INLINE void CVecX::Clamp( float min, float max ) {
	int i;
	for ( i = 0; i < size; i++ ) {
		if ( p[i] < min ) {
			p[i] = min;
		} else if ( p[i] > max ) {
			p[i] = max;
		}
	}
}

Z_INLINE CVecX &CVecX::SwapElements( int e1, int e2 ) {
	float tmp;
	tmp = p[e1];
	p[e1] = p[e2];
	p[e2] = tmp;
	return *this;
}

Z_INLINE float CVecX::Length( void ) const {
	int i;
	float sum = 0.0f;

	for ( i = 0; i < size; i++ ) {
		sum += p[i] * p[i];
	}
	return CZMath::Sqrt( sum );
}

Z_INLINE float CVecX::LengthSqr( void ) const {
	int i;
	float sum = 0.0f;

	for ( i = 0; i < size; i++ ) {
		sum += p[i] * p[i];
	}
	return sum;
}

Z_INLINE CVecX CVecX::Normalize( void ) const {
	int i;
	CVecX m;
	float invSqrt, sum = 0.0f;

	m.SetTempSize( size );
	for ( i = 0; i < size; i++ ) {
		sum += p[i] * p[i];
	}
	invSqrt = CZMath::InvSqrt( sum );
	for ( i = 0; i < size; i++ ) {
		m.p[i] = p[i] * invSqrt;
	}
	return m;
}

Z_INLINE float CVecX::NormalizeSelf( void ) {
	float invSqrt, sum = 0.0f;
	int i;
	for ( i = 0; i < size; i++ ) {
		sum += p[i] * p[i];
	}
	invSqrt = CZMath::InvSqrt( sum );
	for ( i = 0; i < size; i++ ) {
		p[i] *= invSqrt;
	}
	return invSqrt * sum;
}

Z_INLINE int CVecX::GetDimension( void ) const {
	return size;
}

Z_INLINE CVec3 &CVecX::SubVec3( int index ) {
	assert( index >= 0 && index * 3 + 3 <= size );
	return *reinterpret_cast<CVec3 *>(p + index * 3);
}

Z_INLINE const CVec3 &CVecX::SubVec3( int index ) const {
	assert( index >= 0 && index * 3 + 3 <= size );
	return *reinterpret_cast<const CVec3 *>(p + index * 3);
}

Z_INLINE CVec6 &CVecX::SubVec6( int index ) {
	assert( index >= 0 && index * 6 + 6 <= size );
	return *reinterpret_cast<CVec6 *>(p + index * 6);
}

Z_INLINE const CVec6 &CVecX::SubVec6( int index ) const {
	assert( index >= 0 && index * 6 + 6 <= size );
	return *reinterpret_cast<const CVec6 *>(p + index * 6);
}

Z_INLINE const float *CVecX::ToFloatPtr( void ) const {
	return p;
}

Z_INLINE float *CVecX::ToFloatPtr( void ) {
	return p;
}
//*/

//===============================================================
//
//	CPolar3
//
//===============================================================

class CPolar3 {
public:	
	float			radius, theta, phi;

					CPolar3( void );
					explicit CPolar3( const float radius, const float theta, const float phi );

	void 			set( const float radius, const float theta, const float phi );

	float			operator[]( const int index ) const;
	float &			operator[]( const int index );
	CPolar3		operator-() const;
	CPolar3 &		operator=( const CPolar3 &a );

	CVec3			toVec3( void ) const;
};

Z_INLINE CPolar3::CPolar3( void ) {
}

Z_INLINE CPolar3::CPolar3( const float radius, const float theta, const float phi ) {
	assert( radius > 0 );
	this->radius = radius;
	this->theta = theta;
	this->phi = phi;
}
	
Z_INLINE void CPolar3::set( const float radius, const float theta, const float phi ) {
	assert( radius > 0 );
	this->radius = radius;
	this->theta = theta;
	this->phi = phi;
}

Z_INLINE float CPolar3::operator[]( const int index ) const {
	return ( &radius )[ index ];
}

Z_INLINE float &CPolar3::operator[]( const int index ) {
	return ( &radius )[ index ];
}

Z_INLINE CPolar3 CPolar3::operator-() const {
	return CPolar3( radius, -theta, -phi );
}

Z_INLINE CPolar3 &CPolar3::operator=( const CPolar3 &a ) { 
	radius = a.radius;
	theta = a.theta;
	phi = a.phi;
	return *this;
}

Z_INLINE CVec3 CPolar3::toVec3( void ) const {
	float sp, cp, st, ct;
	CZMath::SinCos( phi, sp, cp );
	CZMath::SinCos( theta, st, ct );
 	return CVec3( cp * radius * ct, cp * radius * st, radius * sp );
}


/*
===============================================================================

	Old 3D vector macros, should no longer be used.

===============================================================================
*/
/*
#define dotProduct( a, b)			((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
#define vectorSubtract( a, b, c )	((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
#define vectorAdd( a, b, c )		((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
#define	vectorScale( v, s, o )		((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
#define	vectorMA( v, s, b, o )		((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s))
#define vectorCopy( a, b )			((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
//*/


#endif // CVECTOR_H